/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/task_manager.h
 * @Description  : 任务管理器类，用于管理AGV的任务调度与执行。 
 *                 包含任务的添加、验证、执行、状态更新等功能，支持在线和离线订单处理。
 *                 通过多线程实现任务的循环处理，确保任务的高效执行与状态管理。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:02:18
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once

#include "trajectory.h"
#include "path.h"
#include "ring_queue.h"
#include "work_code.h"
#include "status.h"
#include "trajectory_navigation.h"
#include <map>
#include <vector>
#include <string>
#include <queue>
#include <mutex>
#include <thread>
#include "new_ring_queue.h"
#include "operation.h"
#include <agv_srvs/srv/order.hpp>
#include <agv_msgs/msg/map_node.hpp>
#include <agv_srvs/srv/local_order.hpp>
#include "geometry.h"
#include "force_relocate.h"
#include "offline_scurve_plan.h"

#define ActionParam_TO_STREAM(stream, param) \
    stream << "    key: \"" << param.key << "\"\n"; \
    stream << "    value: \"" << param.value << "\"\n"; \
    stream << "    valuedatatype: \"" << param.valuedatatype << "\""

#define Action_TO_STREAM(stream, action) \
    stream << "    actiontype: \"" << action.actiontype << "\"\n"; \
    stream << "    actionid: \"" << action.actionid << "\"\n"; \
    stream << "    actiondescription: \"" << action.actiondescription << "\"\n"; \
    stream << "    blockingtype: \"" << action.blockingtype << "\"\n"; \
    stream << "    actionparameters: [\n"; \
    for (const auto& param : action.actionparameters) { \
        stream << "      {\n"; \
        stream << "      "; ActionParam_TO_STREAM(stream, param); stream << "\n"; \
        stream << "      }\n"; \
    } \
    stream << "    ]"

#define Node_TO_STREAM(stream, node) \
    stream << "    nodeid: \"" << node.nodeid << "\"\n"; \
    stream << "    sequenceid: " << node.sequenceid << "\n"; \
    stream << "    nodedescription: \"" << node.nodedescription << "\"\n"; \
    stream << "    released: " << (node.released ? "true" : "false") << "\n"; \
    stream << "    actions: [\n"; \
    for (const auto& action : node.actions) { \
        stream << "      {\n"; \
        Action_TO_STREAM(stream, action); \
        stream << "\n      }\n"; \
    } \
    stream << "    ]"

#define Order_TO_STREAM(stream, order) \
    stream << "Order:\n"; \
    stream << "  timestamp: \"" << order.timestamp << "\"\n"; \
    stream << "  serialnumber: \"" << order.serialnumber << "\"\n"; \
    stream << "  orderid: \"" << order.orderid << "\"\n"; \
    stream << "  orderupdateid: " << order.orderupdateid << "\n"; \
    stream << "  zonesetid: \"" << order.zonesetid << "\"\n"; \
    stream << "  nodes: [\n"; \
    for (const auto& node : order.nodes) { \
        stream << "    {\n"; \
        Node_TO_STREAM(stream, node); \
        stream << "\n    }\n"; \
    } \
    stream << "  ]"

// Define stream operators for each type
inline std::ostream& operator<<(std::ostream& os, const agv_msgs::msg::ActionParam& param) {
    ActionParam_TO_STREAM(os, param);
    return os;
}

inline std::ostream& operator<<(std::ostream& os, const agv_msgs::msg::Action& action) {
    Action_TO_STREAM(os, action);
    return os;
}

inline std::ostream& operator<<(std::ostream& os, const agv_msgs::msg::MapNode& node) {
    Node_TO_STREAM(os, node);
    return os;
}

inline std::ostream& operator<<(std::ostream& os, const agv_msgs::msg::Order& order) {
    Order_TO_STREAM(os, order);
    return os;
}

// #define MAX_TASK_TITLE_LEN 64
constexpr int TASK_BUFFER_NUM = 1024; // Size of task code buffer
constexpr int WORK_BUFFER_NUM = 350;  // 80 Size of work code buffer

constexpr int AGV_IS_IDLE = 0;
constexpr int AGV_IS_NAVIGATE = 1;
constexpr int AGV_IS_IN_OP = 2;

// 到点自旋计算取货时旋转角度，卸货时角度，正常休息时角度
class ZoneSpinData
{
 public:
    // amr到点调整角度；角度的类型
    enum class AdjustAngleType {
        AdTypeNormal,
        AdTypePick,
        AdTypeDrop
    };
    
    ZoneSpinData();
    ZoneSpinData(bool spin, const agv_msgs::msg::MapNode &nav_node);
    ZoneSpinData(const agv_msgs::msg::MapNode &nav_node);//只传入任务节点数据，查找是否spin
    // 获取节点需要调整的角度
    float GetAngle();

    // 选择节点角度类型 节点角度；取货角度；卸货角度
    int SelectNodeAngleType();

 public:
    //初版使用是EnableNodeAngle,控制三个角度同时开启
    //所以需要外部传入
    bool zone_spin_;
    agv_msgs::msg::MapNode nav_node_;
 private:
    // 根据到点节点角度类型，返回制定的角度(弧度)
    float GetAdjustAngle(Node node, const int &angle_type);
};

struct LocalOrder {
    agv_msgs::msg::Order order;
    bool recycle;
    LocalOrder() { 
        Clear(); 
    }
    
    void Clear() { 
        order = agv_msgs::msg::Order();
        recycle = false;
        DEBUG_OUT("LocalOrder clear once");
    }

    bool Recycle() { 
        return recycle; 
    }
};

//////////////////////////////////////////////////////////////////////////////
//  interface of class "CTaskManager".
class TaskManager
{
 public:
    // 任务管理器构造函数
    TaskManager();
    // 任务管理器析构函数
    ~TaskManager();
    //  导航主定时器任务线程(除了在timer中，它不应该被其他调用)
    void TimerTask();
    // 设置初始化状态
    void SetInitState(WorkStatusRecord& vhc_status);
    // 开始/停止导航
    void StartNavigation(bool yes = true, bool online = false);
    // 当前是否是空闲
    bool IsIdle();
    // 添加离线/本地订单任务
    bool OnSendOfflineTask(const agv_srvs::srv::LocalOrder::Request &req);
    // 任务是否有效；id是否重复
    bool TaskValid(long long task_id, USHORT sub_task_id);
    // 添加在线订单任务
    bool OnSendOnlineOrderTask(agv_msgs::msg::Order order_task);
    // 更新最近order 消息
    bool UpdateToRcsInfo(agv_msgs::msg::Order order);
    // 外部强制更新当前预编译时的车体位姿
    void UpdataCurDynVirtulPos(const Posture& pst);
    // 设置暂停编译标识
    void SetPauseCompileFlag(bool pause);

    /////////////栈板识别///////////////////
    // 获取原始地图上的起点终点位姿
    bool GetRawMapPath(int start, int end, std::array<Posture, 2> &result);
    // 修正双向地图路段
    bool ModifyDoubleMapPath(int start, int end, std::array<Point, 4> bezier_points);

 private:
    // 侧移平滑过度结构体
    struct SideSmooth {
        bool use_last_end_steer_ang;
        Angle last_end_steer_ang;
        SideSmooth() { Clear(); }
        void Clear() {
            use_last_end_steer_ang = false;
            last_end_steer_ang = Angle();
            DEBUG_OUT("SideSmooth clear once");
        }
    };
    
    // 初始化或预先设置变量值
    void InitValue();
    //  编译订单任务（当前订单缓冲区存在时）
    void CompileOrderTaskProc();
    // 尝试加载一个编译后导航动作任务
    void LoadNewOrderTaskProc();
    // 运行当前加载的工作码 一条路径或一个动作
    void RunCurWorkCodeProc();
    // 其他的特殊操作
    void OtherDoingProc();
    
    // 预先判断是否需要设置一些标志状态
    void PreDoingProc();

    //   获取当前控制周期时间
    float GetCycle();

    // 检测是否修改了当前规划的结束速度值
    // 检测任务拼接时，是否修改了当前停车的速度
    void CheckTaskConnect();

    // 判断是否需要限制速度时，分割两段降速
    void RefrainDivide(double v0, double &v1, double r_vmax, double amax, double jmax, double q0, double &q1, double minQ);

    // 检测最小距离降速停车判断
    void CheckMinDisDecReplan();

    // 是否允许重新规划
    bool CanRePlan();

    // 选择是否使用载货速度
    CurveParamGroup SelectVelLoad();

    // 运行曲线规划
    void RunCurvePlan();

    // 开始一次新的全局规划
    void StartNewGlobalPlan(double vmax, double amax, double jmax, double cycle,
                            double q0, double q1, double v0, double v1);

 private:
    // 创建导航码
    TrajNv* CreateNvObj(Path* path, Node &to_node, float from, float to, bool force_check_make = FALSE);
    // 修正一个导航码的节点结束速度
    void ModifyEndVel(TrajNv* nv, float end_vel);
    //  通过位姿更新虚拟静态位姿
    bool UpdateVirtualStaticPoseByPst(TrajNv* nv, USHORT to_node);
    // 通过目标节点更新虚拟静态位姿
    bool UpdateVirtualStaticPose(TrajNv* nv, USHORT to_node);
    // 通过动态位姿更新虚拟位姿
    bool UpdateVirtDynPose(TrajNv* nv, USHORT at_node, USHORT to_node, float abs_progress);
    // #1 在线编译当前订单任务码(从订单队列里)
    bool OnlineCompileOrderTask();
    // 编译当前轨迹节点码
    bool OnlineCompileNavNodeCode(ZoneSpinData data);
    // 编译当前动作码
    bool OnlineCompileActionCode(std::vector<agv_msgs::msg::Action> actions);
    // 设置任务缓冲区的的结束标识(是否时一个子订单的结束)
    bool SetWorkBufferEndFlag();
    // 编译当前节点到加点轨迹码
    bool CompileNodeToNodeTraj(USHORT to_node, ZoneSpinData data = ZoneSpinData());
    // 编译路径中间到节点轨迹码
    bool CompileMidToNodeTraj(USHORT target_node, ZoneSpinData data = ZoneSpinData());
    // 预留：编译节点到中间路段
    bool CompileNodeToMidTraj(USHORT to_node);
    //	预留：编译中间路径到中间
    bool CompileMidToMidTraj(USHORT to_node);
    // 获取当前需要走到的目标长度(m)
    float GetTargetProgress();
    // 试图在任务节点结束插入自旋轨迹
    bool InsertSpinTurn(TrajNv* compile_nv = nullptr, float adjust_spin_angle = 0.0f);
    // 判断小车是否需要停止(在适配特殊的导航轨迹或方向等)
    bool NeedStop(TrajNv* nv);
    // 检查车辆当前的转向角度是否适合指定的导航
    bool SteerAngleFit(TrajNv* compile_nv);
    // 检查车辆当前的航向角是否适合指定的导航
    bool HeadingFit(TrajNv* nv);
    // 检查车辆的车轮移动方向是否符合指定的导航
    bool WheelMoveDirFit(TrajNv* compile_nv);
    // 是否需要强制重定位根据人工配置的路段属性
    bool NeedRelocateByPathBits();
    // 检测是否有下一次有效轨迹
    void CheckTrajForRelocate();
    // 重定位循环
    void RelocateCycle();
    // 检测是否有路径且检测到路径是否需要强制重定位
    bool HasNextReadyTraj(bool& force_reloacte);
    // 判断工作码的轨迹导航类型
    short TrajGuideType(WorkCode* work_code);
    // 查找工作码缓冲区队头第一个导航工作码
    WorkCode *FindHeadNvWorkCode();
    // 查找工作码缓冲区队头第一个导航工作码拷贝
    WorkCode *FindHeadNvWorkCodeCopy();
    // 查找工作码缓冲区下一个导航工作码
    WorkCode *FindNextNvWorkCode(USHORT& next_pos);
    // 查找工作码缓冲区尾部的工作码
    WorkCode *FindTailWorkCode();
    // 侧移舵是否能反转
    bool SideCanSteerReserve(Trajectory* traj);
    // 侧移舵反转
    bool SideReserveSteer(Trajectory* cur_traj, Angle ang_next_start);
    // 试图反转侧移舵角
    void TryToReverseSideSteerAngle(TrajNv* cur_nv, TrajNv* next_nv, USHORT& next_pos);
    // 加载新的导航对象并准备运行
    void LoadNvObj(TrajNv* cur_nv);
    // 加载新的操作对象并准备运行
    void LoadOpObj(Operation* op);
    // 加载新的动态操作对象并准备运行
    void LoadDynOpObj(Operation* dyn_op);
    // 运行当前导航行走对象
    void RunCurNvObj();
    // 运行当前静态动作对象
    void RunCurOpObj();
    // 运行当前动态动作对象
    void RunCurDynOpObj();
    // 删除导航行走对象
    void DelNvTask();
    // 删除静态动作对象
    void DelOpTask();
    // 删除冬天动作对象
    void DelDynTask();
    // 清除工作缓冲区
    void ClearWorkBuffer();
    // 清除工作缓冲区副本
    void ClearWorkBufferCopy();
    // 清除小车状态信息
    void ClearVhcStatus();
    // 清除所有任务状态和对象
    void ClearAllTask();
    // 任务报错时，强制清除任务id和子ID
    void ForceClearCurTaskId();
    // 更新当前任务ID
    void UpdateCurTaskId();
    // 设置上一次导航轨迹停止
    bool SetLastNvStop(short pre_nv_type, short tail_nv_type);
    // 设置当前的速度等级0~9
    void SetLevel(int level);
    // 是否最终目标点/区域节点自旋
    bool ZoneNodeSpin(const std::string& zone_set_id, agv_msgs::msg::MapNode cur_node);
    // 任务结束点的words属性值
    bool ZoneNodeWords(const std::string& zone_set_id);
    // 订单信息打印输出
    void LogOutCurOrder(agv_msgs::msg::Order order);
    // 计算上一次导航的结束速度
    bool ComputeLastNvEndVel();
    // 处理强制结束当前任务
    void HandleEndTask();

 private:
    // 获取当前归集的实际距离长度
    // float GetActualRange(TrajNv *pCurNv);
    // 路径长度(为计算结束速度)
    // void PathLengthForEv(TrajNv *pCurNv, float &fCurNvEndVel, float &fPathLength, bool &bIgnore, const float &fRange);
    // 修正结束速度为0(为计算结束速度)
    // void ModifyVelZeroForEv(TrajNv *pLastNv, float &fLastNvEndVel, bool &bStopped);
    // 计算上一个轨迹的最大结束速度
    float CalcPreMaxEndVel(TrajNv* cursor_nv, float pre_max_end_vel, float cursor_nv_end_vel, float path_length, bool ignore);
    // 计算路径长度
    // float CalcPathLength(bool &bStopped, TrajNv *cursor_nv, float &fCursorNvEndVel, float &fPathLength, bool &bIgnore);
    // 修正当前游标之前的结束速度
    void ModifyCursorPreEndVel(TrajNv* cursor_pre_nv, TrajNv* cursor_nv, bool& stopped, float& pre_max_end_vel);
    // 侧移使用上一次的结束舵角
    void SideUseLastEndSteerAng(TrajNv* cur_nv);
    // 设置结束速度通过轮向和轨迹类型
    void SetEndVelByMoveDirAndTrajType(TrajNv* cur_nv, TrajNv* next_nv, float& end_vel);
    // 试图根据实际车头方向，修改自旋的方向(只有在规划起始角度为0或小于5度时，才调整，大角度跳过此逻辑)
    void TryModifySpinDir(Trajectory* cur_traj);
    // 挂起导航任务(由于动态动作)
    void SuspendNvTaskForDynOp();
    // 试图尝试停车(由于动态动作)
    void TryToStopForDynOp();
    // 更新工作状态
    void UpdateWorkState();
    // 更新是否开启托盘随动状态
    void UpdateRackFollowUp(TrajNv* cur_nv);
    // 空闲时处理 return: true:是否持续空闲时间大于指定时间
    void WhenIdle();
    // 如果空闲时编译错误任务触发
    bool IdleTryToOccurError(bool occur, const std::string &msg);
    // 检测单点空任务
    bool SingleNodeTask(const agv_msgs::msg::Order &order);
    /////////////栈板识别///////////////////
    // 反转控制点
    bool ReverseCtrlPoints(std::array<Point, 4> &bezier_points);
    // 根据结果修改替换新的路径
    bool ModifyMapPath(int start, int end, std::array<Point, 4> bezier_points, bool modify_end_node);
    // 栈板识别节点判断
    bool IdentifyNode(std::string cargo_node_id, agv_msgs::msg::MapNode start, agv_msgs::msg::MapNode end);
    // 插入识别动作
    bool InsertIdentifyAction(std::string identify_node_id, std::string cargo_node_id);
    // 替换末端路径
    bool ReplaceTheEndPath(int start, int end);
    // 替换新的生成末端路径
    TrajNv *AddNodeToNodeTraj(USHORT from_node, USHORT to_node);
    // 末端路径新生成对象
    TrajNv *AddNvObj(Path *path, Node &node_to, float from, float to, bool force_check_mark = FALSE);
    // 
    bool ReplacePalletEndPath(int start, int end, std::array<Point, 4> bezier_points);
    //
    TrajNv *AddNodeToNodeTrajWithPath(Node &from_node, Node &to_node, Path *path);
 private:
    // 本地调试自动添加循环任务线程
    static void ThreadFun(TaskManager* task_manager);
    // 循环任务函数
    void ThreadCycle();
    // 线程对象
    std::thread thread_;
    // 退出线程锁
    std::timed_mutex quit_mutex_;
    // 定时器线程锁
    // CCriticalSection m_crit;
    // std::mutex m_timer_mutex;

    // 本地调试当前订单
    LocalOrder local_order_;
    // 订单缓冲区
    NewRingQueue<agv_msgs::msg::Order> order_task_buffer_;
    // 校验任务ID是否重复缓冲区
    RingQueue<TaskId> task_id_buffer_;
    // 待执行任务缓冲区
    RingQueue<WorkCode> work_buffer_;
    // 待执行任务缓冲区拷贝(计算轨迹规划速度使用)
    RingQueue<WorkCode> work_buffer_copy_;
    // 当前正在执行的任务(行走或静态动作)
    WorkCode cur_work_code_;
    // 任务状态标识 0 - normal; 1 - Start; 2 - end
    UCHAR task_status_;
    // 当前的速度等级
    short level_;
    // 曲率是否已知
    bool curvature_known_;
    // 下一个路径的开始曲率
    float curvature_;
    // 下一个路径的开始舵角度
    Angle last_steer_angle_;
    // 记录上一次的导航速度
    Velocity record_last_nv_vel_;
    // 当前的规划模式
    short profile_mode_;
    // 自旋方向；总是智能旋转方式：SMART_TURN_DIR
    int spin_turn_dir_;
    // 是否使用速度等级
    bool use_speed_level_;
    // 当前执行订单的ID
    long long cur_task_id_;
    // 当前订单的子ID
    USHORT cur_sub_task_id_;
    // 工作状态
    USHORT work_state_;
    // 上一次的轨迹类型
    int last_traj_type_;
    // 是否能编译一个新的任务
    bool can_compile_one_new_task_;
    // 是否强制结束当前任务标识
    bool lc_end_task_;
    // 动态虚拟状态(预编译使用，编译导航任务完成时，更新一次) CompileTrajCode.CreateNvObj finished to update it
    WorkStatusRecord dyn_virt_status_;
    // 当前正在执行的行走任务指针
    TrajNv* cur_nv_;
    // 当前正在执行的静态任务指针
    Operation* cur_op_;
    // 当前正在执行的动态任务指针
    Operation* cur_dyn_op_;
    // 是否由于动态动作未在制定节点前完成；导致编加载新的任务时，暂时挂起
    bool suspend_for_dyn_op_;
    // 订单互斥锁
    std::mutex order_mutex_;
    // 是否时在线/离线(本地)
    bool online_;
    // 导航是否开始运行
    bool start_;
    // 运行对象互斥锁
    std::mutex run_obj_mutex_;
    // 暂停当前预编译阶段;防止原地矫正后车头与预先编译的车头不一致导致出现规划错误
    bool pause_cur_compile_;
    // 编译任务错误时-任务错误描述
    std::string task_err_detail_;
    // 侧移舵角平滑过度处理变量
    SideSmooth side_sooth_;
    // 二维码切换激光时，判断是否效果不好，自动重定位一次
    ForceRelocate relocate_;
    // 空闲起步时，发送起步几秒状态
    IdleToRun idle_to_run_;
    // 全局曲线规划
    OfflineScurvePlan global_plan_;
};

